#pragma warning(disable : 4786)

#include "particleSystem.h"
#include "vl/VLMath.h"
#include "ForceGravity.h"
#include "ConstraintCircle.h"
#include <cstdio>
#include <cstdlib>
#include <cassert>
#include <cmath>

//#include <limits>

using namespace std;

static float prevT;

/***************
 * Constructors
 ***************/

ParticleSystem::ParticleSystem() 
{	
	n = 0;
	nPerFrame = 5;
	mouseForceIsOn = false;
	mouseForce[0] = 0.0;
	mouseForce[1] = 0.0;
	mouseForce[2] = 0.0;
	forceGravity = new ForceGravity();

	//forces = new Force*[FORCE_TYPE_COUNT];
	//forces[FORCE_TYPE_GRAVITY] = new ForceGravity();

	//constraints = new Constraint*[CONSTRAINT_TYPE_COUNT];
	//constraints[CONSTRAINT_TYPE_CIRCLE] = new ConstraintCircle();

	// Emitt particle
	emitBall();
	emitBall2();
	
	constraintCircle = new ConstraintCircle();
	constraintDistance = new ConstraintDistance();
}





/*************
 * Destructor
 *************/

ParticleSystem::~ParticleSystem() 
{
	// TODO
	//delete forces;
	//delete constraints;

}


/******************
 * Simulation fxns
 ******************/

/** Start the simulation */
void ParticleSystem::startSimulation(float t)
{
    // TODO

	// These values are used by the UI ...
	// negative bake_end_time indicates that simulation
	// is still progressing, and allows the
	// indicator window above the time slider
	// to correctly show the "baked" region
	// in grey.
	bake_end_time = -1;
	simulate = true;
	dirty = true;

}

/** Stop the simulation */
void ParticleSystem::stopSimulation(float t)
{
    
	// TODO

	// These values are used by the UI
	simulate = false;
	dirty = true;

}

/** Reset the simulation */
void ParticleSystem::resetSimulation(float t)
{
    
	// TODO

	// These values are used by the UI
	simulate = false;
	dirty = true;

}

/** Compute forces and update particles **/
void ParticleSystem::computeForcesAndUpdateParticles(float t)
{
	// TODO
	currentTime = t;
	if (simulate) {
		
		list<Particle>::iterator p;
		for(p = particlesList.begin(); p != particlesList.end(); ++p) {
			//Clear forces
			p->resetForce();

			//Gravity:	
			forceGravity->applyForce2(*p, gravity);
			
			//Mouse Force
			computeMouseForce();
		
			//Apply Constraints
			/*for(int c = 0; c < CONSTRAINT_TYPE_COUNT; ++c) {
				constraints[c]->applyConstraint(*p, Ks, Kd);
			}*/
		}

		//APPLY INDIVIDUAL CONSTRAINTS
		constraintCircle->applyConstraint(particlesList.front(), 0.0, 0.0, Ks, Kd);

		//testConstraintCircle();
		/*constraintDistance->applyConstraint(particlesList.back(), 
			particlesList.front().getPosition()[0], particlesList.front().getPosition()[1], Ks, Kd);
*/
		constraintDistance->applyConstraint2(particlesList.back(), particlesList.front(), Ks, Kd);

		//SOLVER (euler):
		eulerSolver(t-prevT);
	}
	
	// Debugging info
	if( t - prevT > .04 )
		printf("(!!) Dropped Frame %lf (!!)\n", t-prevT);
	prevT = t;
}

void ParticleSystem::testConstraintCircle()
{
	Particle p = particlesList.front();
	Matd J(1,3, p.getVelocity()[0], p.getVelocity()[1], 0.0);

	Matd W(3,3, p.getMass(), 0.0, 0.0, 0.0, p.getMass(), 0.0, 0.0, 0.0, p.getMass()); 

	Matd Jd(1,3, p.getForce()[0]/p.getMass(), p.getForce()[1]/p.getMass(), 0.0);
	
	Matd qd(3,1, p.getVelocity()[0], p.getVelocity()[1], 0.0);

	//Vec3f Q = p.getForce();
	Matd Q(3,1, p.getForce()[0], p.getForce()[1], 0.0);

	Matd l = inv(J*W* trans(J)) -J*qd - J*W*Q;

	Vecd f = inv(J)*l[0];

	//Matd rightSide1 = Jd*qd;



	std::cout << "f: " << f << std::endl;
}


void ParticleSystem::eulerSolver(float dt)
{
	list<Particle>::iterator p;
	for(p = particlesList.begin(); p != particlesList.end(); ++p) {
		p->setPosition( p->getPosition() + (dt * p->getVelocity()) );
		p->setVelocity( p->getVelocity() + (dt * (p->getForce()/p->getMass()) ) );
	}
}

void ParticleSystem::setKs(float k)
{
	Ks = k;
}

void ParticleSystem::setKd(float k)
{
	Kd = k;
}

void ParticleSystem::setGravity(float g)
{
	gravity = g;
}

void ParticleSystem::emitBall()
{
	Particle p;
	p.setPosition(5.0, 0.0, 0.0);
	p.setVelocity(0.0, 0.0, 0.0);
	p.setMass(4.0);
	p.setSize(0.5);
	p.resetForce();
	particlesList.push_back(p);
}

void ParticleSystem::emitBall2()
{
	Particle p;
	p.setPosition(8.0, 0.0, 0.0);
	p.setVelocity(0.0, 0.0, 0.0);
	p.setMass(4.0);
	p.setSize(0.5);
	p.resetForce();
	particlesList.push_back(p);
}



/** Render particles */
void ParticleSystem::drawParticles(float t)
{
    list<Particle>::iterator p;    
    for(p = particlesList.begin(); p != particlesList.end(); ++p) {
        p->draw();
    }	
}

void ParticleSystem::mouseClick(int x, int y)
{
	clickTime = currentTime;
	mousePoint[0] = x*0.0834 - 14.178; //Assuming Screen W=340
	mousePoint[1] = 12.51 - y*0.0834; //Assuming Screen H=300
	mousePoint[2] = 0.0;
	mouseForceIsOn = true;
	//std::cout << "mousePoint " << mousePoint << " Time: " << currentTime << std::endl;
}

void ParticleSystem::mouseRelease()
{
	mouseForceIsOn = false;
}

void ParticleSystem::computeMouseForce()
{
	if(mouseForceIsOn) {
		float dTime = currentTime - clickTime;
		float Km = 80.0;
		float Kd = 2.0;
		Vec3f Fdirection = mousePoint - particlesList.front().getPosition();
		Fdirection = norm(Fdirection);
		Vec3f Fm = Fdirection*(Km);
		Vec3f Fd = -Kd*particlesList.front().getVelocity();
		Vec3f F = Fm + Fd;
		//std::cout << "F " << F << std::endl;
		particlesList.front().addForce(F[0], F[1], 0.0);
	}
}


/** Adds the current configuration of particles to
  * your data structure for storing baked particles **/
void ParticleSystem::bakeParticles(float t) 
{

	// TODO (baking is extra credit)
}

/** Clears out your data structure of baked particles */
void ParticleSystem::clearBaked()
{

	// TODO (baking is extra credit)
}
